#include <glog/logging.h>
#include <QFileInfo>
#include <QSettings>
#include <QTime>
#include <QDir>

#include "logger.h"
#include "positonModule.h"
#include "DetectCover.h"

#define IMG_2D_PATH "chargeRobot/Image2D"
#define IMG_3D_PATH "chargeRobot/Image3D"
//int dh_access[2] = {4, 4};
//std::string leftMac{"00-21-49-03-6F-20"};
//std::string rightMac{"00-21-49-03-13-4D"};

//int exposure_Left   = 100000;
//int exposure_Right  = 100000;

PositonModule::PositonModule()
{
    createImgFolder();
    m_mech = new Camera_mech();
}

PositonModule::~PositonModule()
{
    if(m_mech)
    {
        m_mech->closeDevice ();
        delete m_mech;
        m_mech = nullptr;
    }
}

int PositonModule::init()
{
    if (cover_detecter.init())
    {
        LOG(ERROR) << "plate_detecter.init() err!" << std::endl;
        return -1;
    }
    return 0;
}

bool PositonModule::softTriggerAll(cv::Mat &img, bool save)
{
    cv::Mat imgX,imgY,imgZ;
    if(m_mech->is_connected ())
    {
        //fl0929++ 不修改3d参数，如果修改，会导致version参数被修改
        //应该备份这个值，设置完毕后，再复原
        //m_mech->set3D4Ident(false); //false
        if(!m_mech->capture_color_point(img,imgX,imgY,imgZ))
        {
            LOG(ERROR) << "Mech capture colorMap Failed!!\n";
            return false;
        }
    }
    else{
        LOG(ERROR) << "Mech Dev is Disconnected!!\n";
        return false;
    }

    if(save){
        QDateTime time = QDateTime::currentDateTime();
        QString Path =  QString("%1/%2").arg(IMG_3D_PATH).arg(time.toString("yyyyMMdd"));

        QString fileName = QString("%1-mech.png").arg(time.toString("HHmmss"));
        QString fileNameX = QString("%1-cloudX.tif").arg(time.toString("HHmmss"));
        QString fileNameY = QString("%1-cloudY.tif").arg(time.toString("HHmmss"));
        QString fileNameZ = QString("%1-cloudZ.tif").arg(time.toString("HHmmss"));

        QString fullPath = QString("%1/%2").arg(Path).arg(fileName);
        QString fullPathX = QString("%1/%2").arg(Path).arg(fileNameX);
        QString fullPathY = QString("%1/%2").arg(Path).arg(fileNameY);
        QString fullPathZ = QString("%1/%2").arg(Path).arg(fileNameZ);

        saveIMG(fullPath, img);
        saveIMG(fullPathX,imgX);
        saveIMG(fullPathY,imgY);
        saveIMG(fullPathZ,imgZ);
    }

    return true;
}
bool PositonModule::softTriggerMECH(cv::Mat &img, bool save)
{
    if(m_mech->is_connected ())
    {
        //fl0929++ 不修改3d参数，如果修改，会导致version参数被修改
        //应该备份这个值，设置完毕后，再复原
        //m_mech->set3D4Ident(false); //false

        if(bUseFlash){
            softTriggerAll(img, false);
        }
        if(!m_mech->capture_colorMap(img))
        {
            LOG(ERROR) << "Mech capture colorMap Failed!!\n";
            return false;
        }
    }
    else{
        LOG(ERROR) << "Mech Dev is Disconnected!!\n";
        return false;
    }

    if(save){
        QDateTime time = QDateTime::currentDateTime();
        QString Path =  QString("%1/%2").arg(IMG_2D_PATH).arg(time.toString("yyyyMMdd"));
        QString fileName = QString("%1-mech.png").arg(time.toString("HHmmss"));

        QString fullPath = QString("%1/%2").arg(Path).arg(fileName);

        saveIMG(fullPath, img);
    }
    return true;
}

bool PositonModule::connectCam()
{
    if(true == mInited){
        return true;
    }
    if(m_mech)
    {
        if(!m_mech->openDevice ())
        {
            LOG(ERROR) << " mechmind openDevice failed\n";
            return false;
        }
    }
    mInited = true;
    return mInited;
}

void PositonModule::createImgFolder()
{
    QDir dir;
    dir.setPath(IMG_2D_PATH);
    if(!dir.exists())
    {
        bool ismkdir = QDir().mkdir(IMG_2D_PATH);
        if(!ismkdir)
            LOG(WARNING) << "Create ./Image2D Failed!!\n";
    }
}

bool PositonModule::saveIMG(QString fullpath, cv::Mat &img)
{
    QStringList folders = fullpath.split("/");
    if(folders.size() == 4)
    {
        QString Path =  QString("%1/%2/%3").arg(folders.at(0)).arg(folders.at(1)).arg(folders.at(2));
        QDir dir(Path);
        if(!dir.exists())
        {
            bool ismkdir = QDir().mkdir(Path);
            if(!ismkdir)
            {
                LOG(ERROR) << "Create "<< Path.toStdString().c_str() << " failed!!\n";
                return false;
            }
        }

        if(!cv::imwrite(fullpath.toStdString().c_str(), img))
        {
            LOG(ERROR) << "save image failed!! fileName:" << fullpath.toStdString().c_str() << std::endl;
            return false;
        }
        return true;
    }
    else
    {
        LOG(ERROR) << "Invalid Path!! recv:" << fullpath.toStdString().c_str() << std::endl;
        return false;
    }
}

void PositonModule::loadSysIni()
{
#if 1
    QFileInfo fileInfo(CONFIG_FILE_PATH);
    if(fileInfo.isFile())
    {
        QSettings setting(CONFIG_FILE_PATH, QSettings::IniFormat);
        bUseFlash = setting.value("/POS/use_flash").toBool();
    }

    LOG(INFO) << "bUseFlash =" << bUseFlash<<std::endl;
#endif
}

bool PositonModule::identCoverPos(cv::Mat &img,cv::Rect& rect, float &outx, float &outy, float &outz)
{
    return cover_detecter.findCoverDistance(img, rect, outx, outy, outz);
}

bool PositonModule::coverPos3D(cv::Mat &img,cv::Rect &rect,std::vector<float>& result)
{

    if(!cover_detecter.yoloDetect(img,rect))
    {
        return false;
    }

    std::cout << "rect" << rect <<std::endl;
    if( m_mech->detect_cloud_rect_xyz(rect,result)!=0)
    {
        return false;
    }

    std::cout <<"result size=" <<result.size()<<std::endl;
    return true;
}
